#include "Camera.h"

const FLOAT MIN_DISTANCE		= 5.0f;
const FLOAT MAX_DISTANCE		= 25.0f;

const FLOAT PHI_CLAMP_ANGLE		= 176.0f;		// Camera can rotate 176 degrees vertically.

//=====================================================================================
// Constructors
//=====================================================================================

//-------------------------------------------------------------------------------------
Camera::Camera() :
	m_character			(nullptr),
	m_characterRotY		(0),
	m_distance			(10.0f), 
	m_theta				(-90.0f*(XM_PI/180.0f)),
	m_phi				(45.0f*(XM_PI/180.0f)),  
	m_targetOffset		(-2.0f),
	m_bThirdPerson		(true),
	m_fpRight			(XMFLOAT3(1.0f, 0.0f, 0.0f)),
	m_fpUp				(XMFLOAT3(0.0f, 1.0f, 0.0f)),
	m_fpLook			(XMFLOAT3(0.0f, 0.0f, 1.0f)),
	m_characterLook		(XMFLOAT3(0.0f, 0.0f, 0.0f)),
	m_characterRight	(XMFLOAT3(0.0f, 0.0f, 0.0f)),
	m_characterUp		(XMFLOAT3(0.0f, 0.0f, 0.0f))
{
	SetLens(0.25*XM_PI, 1.0f, 1.0f, 1000.0f);				// Create projection matrix.

	m_Frustum				= new XNA::Frustum;				// Create camera frustum.
	
	// Setup the up and look basis vectors about the right basis vector.
	float phiOffset			= 30.0f*(XM_PI/180.0f);
	XMMATRIX R				= XMMatrixRotationAxis(XMLoadFloat3(&m_fpRight), phiOffset);
	
	XMStoreFloat3(&m_fpUp,	 XMVector3TransformNormal(XMLoadFloat3(&m_fpUp),   R));
	XMStoreFloat3(&m_fpLook, XMVector3TransformNormal(XMLoadFloat3(&m_fpLook), R));
}

//-------------------------------------------------------------------------------------
Camera::~Camera()
{
	SafeDelete(m_Frustum);
}

//=====================================================================================
// Public Interface Methods
//=====================================================================================

//-------------------------------------------------------------------------------------
void Camera::Initialise()
{
	// Claculate clamping angle for the third-person and first-person systems.
	float phiMag = PHI_CLAMP_ANGLE;
	float offset = 2.0f;

	m_tpPhiClampMin = phiMag - (phiMag - offset);
	m_tpPhiClampMax = phiMag + offset;

	m_fpPhiClampMin = (phiMag * 0.5f) * -1;
	m_fpPhiClampMax = phiMag * 0.5f;
}

//-------------------------------------------------------------------------------------
void Camera::UpdateView()
{
	if (m_bThirdPerson)				// If camera is in third-person mode.
	{
		TPUpdateView();				// Updated view matrix using third-person algorithm.
	}
	else							// Otherwise...
	{
		FPUpdateView();				// Updated view matrix using first-person algorithm.
	}
}

//-------------------------------------------------------------------------------------
void Camera::UpdateRotation(float theta, float phi)
{
	m_theta		-= theta;
	m_phi		-= phi;

	TPUpdateRotation();				// Update third-person camera rotation.

	FPUpdateRotation(theta, phi);	// Update first-person camera rotation.
}

//-------------------------------------------------------------------------------------
void Camera::UpdateDistance(float d)
{
	if (m_bThirdPerson)							// If camera is in third-person mode.
	{
		if ((m_distance + d) < MIN_DISTANCE)	// If the desired distance surpasses the minimum clamp value.
		{
			m_distance = MIN_DISTANCE;			// Set distance to the minimum clamp value.
			return;
		}

		if ((m_distance + d) > MAX_DISTANCE)	// If the desired distance surpasses the maximum clamp value.
		{
			m_distance = MAX_DISTANCE;			// Set distance to the maximum clamp value.
			return;
		}

		m_distance += d;		// Update the camera's distance from the character.
	}
}

//-------------------------------------------------------------------------------------
void Camera::Walk(float d)
{
	if (m_bThirdPerson)					// If camera is in third-person mode.
	{
		TPWalk(d);						// Call third-person Walk.
	}
	else								// Otherwise...
	{
		FPWalk(d);						// Call first-person Walk.
	}
}

//-------------------------------------------------------------------------------------
void Camera::Walk(float d, XMFLOAT3& currentVelocity)
{
	if (m_bThirdPerson)					// If camera is in third-person mode.
	{
		TPWalk(d, currentVelocity);		// Call third-person Walk - based on character's velocity.
	}
	else								// Otherwise...
	{
		FPWalk(d, currentVelocity);		// Call third-person Walk - based on character's velocity.
	}
}

//-------------------------------------------------------------------------------------
void Camera::Strafe(float d)
{
	if (m_bThirdPerson)					// If camera is in third-person mode.
	{
		TPStrafe(d);					// Call third-person Walk.
	}
	else								// Otherwise...
	{
		FPStrafe(d);					// Call first-person Walk.
	}
}

//-------------------------------------------------------------------------------------
void Camera::Strafe(float d, XMFLOAT3& currentVelocity)
{
	if (m_bThirdPerson)					// If camera is in third-person mode.
	{
		TPStrafe(d, currentVelocity);	// Call third-person Strafe - based on character's velocity.
	}
	else								// Otherwise...
	{
		FPStrafe(d, currentVelocity);	// Call third-person Strafe - based on character's velocity.
	}
}

//-------------------------------------------------------------------------------------
void Camera::ToggleThirdPerson()
{
	if (!m_bThirdPerson)				// If camera is NOT in third-person mode.
	{
		m_bThirdPerson = true;			// Set third person flag to true.
	}
}

//-------------------------------------------------------------------------------------
void Camera::ToggleFirstPerson()
{
	if (m_bThirdPerson)					// If camera IS in third-person mode.
	{
		m_bThirdPerson = false;			// Set third person flag to false.
	}
}

//-------------------------------------------------------------------------------------
bool Camera::IsThirdPerson() const
{
	return m_bThirdPerson;				// Return third person flag.
}

//=====================================================================================
// Setters
//=====================================================================================

//-------------------------------------------------------------------------------------
void Camera::SetCharacter(BasicMesh *character)
{
	m_character = character;
}

//-------------------------------------------------------------------------------------
void Camera::SetLens(float fov_y, float aspect, float zn, float zf)
{
	// Calculate projection matrix and store in class for retrieval.
	XMMATRIX P = XMMatrixPerspectiveFovLH(fov_y, aspect, zn, zf);
	XMStoreFloat4x4(&m_proj, P);
}

//-------------------------------------------------------------------------------------
void Camera::SetTargetOffset(float offset)
{
	MathHelper::Clamp(offset, 0.0f, 6.0f);	// Target offset clamped.

	m_targetOffset = offset;
}

//-------------------------------------------------------------------------------------
void Camera::SetDistance(float distance)
{
	m_distance = distance;
}

//=====================================================================================
// Getters
//=====================================================================================

//-------------------------------------------------------------------------------------
XMFLOAT3 Camera::GetPosition() const
{
	return m_position;
}

//-------------------------------------------------------------------------------------
float Camera::GetTargetOffset() const
{
	return m_targetOffset;
}

//-------------------------------------------------------------------------------------
float Camera::GetDistance() const
{
	return m_distance;
}

//-------------------------------------------------------------------------------------
XNA::Frustum* Camera::GetFrustum() const
{
	return m_Frustum;
}

//-------------------------------------------------------------------------------------
XMMATRIX Camera::View() const
{
	return XMLoadFloat4x4(&m_view);
}

//-------------------------------------------------------------------------------------
XMMATRIX Camera::Proj() const
{
	return XMLoadFloat4x4(&m_proj);
}

//-------------------------------------------------------------------------------------
XMMATRIX Camera::ViewProj() const
{
	return XMMatrixMultiply(View(), Proj());
}

//=====================================================================================
// Third-Person Mode Methods [private]
//=====================================================================================

//-------------------------------------------------------------------------------------
void Camera::TPUpdateView()
{
	float camX, camY, camZ;

	// Convert Spherical to Cartesian coordinates based on sphere and character position.

	camX = m_distance * sinf(m_phi) * cosf(m_theta);		// Camera cartesian X position.

	camZ = m_distance * sinf(m_phi) * sinf(m_theta);		// Camera cartesian Z position.
	
	camY = m_distance * cosf(m_phi) + m_targetOffset;  // NOTE: Add offset from character. Y position.

	// Add the character world X and Z positions to corresponding positions.

	XMFLOAT3 character_world = m_character->GetTranslation();

	float targetX = character_world.x;

	float targetY = character_world.y;

	float targetZ = character_world.z;

	// Update the camera world position by adding the character's X and Z world position.
	
	camX += targetX;

	camZ += targetZ;

	// Build the view matrix.

	XMVECTOR pos	= XMVectorSet(camX, camY, camZ, 1.0f);

	XMVECTOR target = XMVectorSet(targetX, targetY, targetZ, 0.0f);

	XMVECTOR up		= XMVectorSet(0.0f, 1.0f, 0.0f, 0.0f);

	XMMATRIX V	= XMMatrixLookAtLH(pos, target, up);

	XMStoreFloat4x4(&m_view, V);

	// Update position for application's eye vector for HLSL.

	m_position = XMFLOAT3(camX, camY, camZ);
}

//-------------------------------------------------------------------------------------
void Camera::TPUpdateRotation()
{
	//	Clamp third-person phi angle.

	float phiDeg;	
	
	phiDeg	= m_phi * (180.0f / XM_PI);

	phiDeg	= MathHelper::Clamp(phiDeg, m_tpPhiClampMin, m_tpPhiClampMax);
	m_phi	= phiDeg * (XM_PI / 180.0f);
}

//-------------------------------------------------------------------------------------
void Camera::TPStrafe(float d)
{
	// Calculate character look vector.

	float m_radianRotY = m_characterRotY * (XM_PI / 180.0f);

	XMMATRIX mR = XMMatrixRotationY(m_radianRotY);					// Create rotation matrix.

	XMVECTOR vZ = XMLoadFloat3(&XMFLOAT3(0.0f, 0.0f, -1.0f));		// Vector pointing in Z.

	XMVECTOR vL = XMVector3Normalize(XMVector3Transform(vZ, mR));	// Strafe look direction.

	// Calculate new character position.

	XMVECTOR vD = XMVectorReplicate(d);								// Replicate d in the 4 slots.

	XMFLOAT3 P  = m_character->GetTranslation();					// Get character position.

	XMVECTOR vP = XMLoadFloat3(&P);									// Load character position.

	vP = XMVectorMultiplyAdd(vD, vL, vP);							// Calculate new character position.

	XMStoreFloat3(&P, vP);											// Store new character position.

	// Apply to character.

	m_character->SetTranslation(P.x, P.y, P.z);
}

//-------------------------------------------------------------------------------------
void Camera::TPStrafe(float d, XMFLOAT3& currentVelocity)
{
	// Calculate character look vector.

	float m_radianRotY = m_characterRotY * (XM_PI / 180.0f);

	// TODO: Make character's look vector class member.

	XMMATRIX mR = XMMatrixRotationY(m_radianRotY);					// Create rotation matrix.

	XMVECTOR vZ = XMLoadFloat3(&XMFLOAT3(0.0f, 0.0f, -1.0f));		// Vector pointing in Z.

	XMVECTOR vL = XMVector3Normalize(XMVector3Transform(vZ, mR));	// Strafe look direction.

	// Calculate new character position.

	XMVECTOR vD = XMVectorReplicate(d);								// Replicate d in the 4 slots.

	XMFLOAT3 P  = m_character->GetTranslation();					// Get character position.

	XMVECTOR XMVoldPosition = XMLoadFloat3(&P);						// Store old position

	XMVECTOR vP = XMLoadFloat3(&P);									// Load character position.

	XMVECTOR XMVnewPosition = XMVectorMultiplyAdd(vD, vL, vP);		// Calculate new character position.

	XMVECTOR XMVnewVelocity = XMVectorSubtract(XMVnewPosition, XMVoldPosition);	// Calculate new character velocity

	XMVECTOR XMVcurrentVelocity = XMLoadFloat3(&currentVelocity);

	XMVcurrentVelocity = XMVectorAdd(XMVcurrentVelocity, XMVnewVelocity);

	XMStoreFloat3(&currentVelocity, XMVcurrentVelocity);
}

//-------------------------------------------------------------------------------------
void Camera::TPWalk(float d)
{
	// Snap character rotation with camera's rotation.
	
	m_characterRotY = (m_theta * (180.0f / XM_PI));

	m_characterRotY *= -1;

	m_character->SetRotation(0.0f, m_characterRotY, 0.0f);

	// Calculate character look vector.
	
	float m_radianRotY = m_characterRotY * (XM_PI / 180.0f);		// Character rotation into radians.

	XMMATRIX mR = XMMatrixRotationY(m_radianRotY);					// Create rotation matrix.

	XMVECTOR vX = XMLoadFloat3(&XMFLOAT3(-1.0f, 0.0f, 0.0f));		// Vector pointing in X. Z for strafing!

	XMVECTOR vL = XMVector3Normalize(XMVector3Transform(vX, mR));	// Walk look direction.

	// Calculate new character position.

	XMVECTOR vD = XMVectorReplicate(d);								// Replicate d in the 4 slots.

	XMFLOAT3 P  = m_character->GetTranslation();					// Get character position.

	XMVECTOR vP = XMLoadFloat3(&P);									// Load character position.

	vP = XMVectorMultiplyAdd(vD, vL, vP);							// Calculate new character position.

	XMStoreFloat3(&P, vP);											// Store new character position.

	// Apply to character.

	m_character->SetTranslation(P.x, P.y, P.z);
}

//-------------------------------------------------------------------------------------
void Camera::TPWalk(float d, XMFLOAT3& currentVelocity)
{
	// Snap character rotation with camera's rotation.
	
	m_characterRotY = (m_theta * (180.0f / XM_PI));

	m_characterRotY *= -1;

	m_character->SetRotation(0.0f, m_characterRotY, 0.0f);

	// Calculate character look vector.
	
	float m_radianRotY = m_characterRotY * (XM_PI / 180.0f);		// Character rotation into radians.

	XMMATRIX mR = XMMatrixRotationY(m_radianRotY);					// Create rotation matrix.

	XMVECTOR vX = XMLoadFloat3(&XMFLOAT3(-1.0f, 0.0f, 0.0f));		// Vector pointing in X. Z for strafing!

	XMVECTOR vL = XMVector3Normalize(XMVector3Transform(vX, mR));	// Walk look direction.

	// Calculate new character position.

	XMVECTOR vD = XMVectorReplicate(d);								// Replicate d in the 4 slots.

	XMFLOAT3 P  = m_character->GetTranslation();					// Get character position.

	XMVECTOR XMVoldPosition = XMLoadFloat3(&P);						// Store old position

	XMVECTOR vP = XMLoadFloat3(&P);									// Load character position.

	XMVECTOR XMVnewPosition = XMVectorMultiplyAdd(vD, vL, vP);		// Calculate new character position.

	XMVECTOR XMVnewVelocity = XMVectorSubtract(XMVnewPosition, XMVoldPosition);	// Calculate new character velocity

	XMVECTOR XMVcurrentVelocity = XMLoadFloat3(&currentVelocity);

	XMVcurrentVelocity = XMVectorAdd(XMVcurrentVelocity, XMVnewVelocity);

	XMStoreFloat3(&currentVelocity, XMVcurrentVelocity);
}

//=====================================================================================
// First-Person Mode Methods [private]
//=====================================================================================

//-------------------------------------------------------------------------------------
void Camera::FPUpdateView()
{
	// Load in the camera's basis vectors.
	XMVECTOR R	= XMLoadFloat3(&m_fpRight);
	XMVECTOR U	= XMLoadFloat3(&m_fpUp);
	XMVECTOR L	= XMLoadFloat3(&m_fpLook);

	// Load in the camera's position. (based on character position)

	XMVECTOR vOffset  = XMVectorSet(1.5f, 1.5f, 1.5f, 1.5f);			// Camera offset from character.
	XMVECTOR P = XMLoadFloat3(&m_character->GetTranslation());			// Get character's position.
	XMStoreFloat3(&m_position, XMVectorMultiplyAdd(vOffset, L, P));		// Calculate camera position.

	// Keep camera's axes orthogonal to each other and of unit length.
	L = XMVector3Normalize(L);
	U = XMVector3Normalize(XMVector3Cross(L, R));
	R = XMVector3Cross(U, L);

	// Fill in the view matrix entries.
	float x = -XMVectorGetX(XMVector3Dot(P, R));	// (position DOT r).x
	float y = -XMVectorGetX(XMVector3Dot(P, U));	// (position DOT y).y
	float z = -XMVectorGetX(XMVector3Dot(P, L));	// (position DOT z).z

	XMStoreFloat3(&m_fpRight,	R);
	XMStoreFloat3(&m_fpUp,		U);
	XMStoreFloat3(&m_fpLook,	L);

	m_view(0,0)		= m_fpRight.x;
	m_view(1,0)		= m_fpRight.y;
	m_view(2,0)		= m_fpRight.z;
	m_view(3,0)		= x;

	m_view(0,1)		= m_fpUp.x;
	m_view(1,1)		= m_fpUp.y;
	m_view(2,1)		= m_fpUp.z;
	m_view(3,1)		= y - 2.0f;		// Move camera up to nice height.

	m_view(0,2)		= m_fpLook.x;
	m_view(1,2)		= m_fpLook.y;
	m_view(2,2)		= m_fpLook.z;
	m_view(3,2)		= z;

	m_view(0,3)		= 0.0f;
	m_view(1,3)		= 0.0f;
	m_view(2,3)		= 0.0f;
	m_view(3,3)		= 1.0f;

	
	/* The view matrix is structured as follows: (the inverse of the camera's world matrix)

	Ux		Vx		Wx		0
	Uy		Vy		Wy		0
	Uz		Vz		Wz		0
	-Q.u    -Q.v    -Q.w		1

	where
		Q = world origin of camera
		U = camera x-axis basis vector
		V = camera y-axis basis vector
		W = camera z-axis basis vector													*/
}

//-------------------------------------------------------------------------------------
void Camera::FPUpdateRotation(float theta, float phi)
{
	// Rotate the basis vectors about the world y-axis. (theta)
	XMMATRIX R = XMMatrixRotationY(theta);

	XMStoreFloat3(&m_fpRight,	XMVector3TransformNormal(XMLoadFloat3(&m_fpRight),	R));
	XMStoreFloat3(&m_fpUp,		XMVector3TransformNormal(XMLoadFloat3(&m_fpUp),		R));
	XMStoreFloat3(&m_fpLook,	XMVector3TransformNormal(XMLoadFloat3(&m_fpLook),	R));
	
	//	Clamp First-Person Phi Angle (Y basis vector)
	
	// Store the previous frame values.
	XMVECTOR tempUp = XMVectorSet(0.0f, 1.0f, 0.0f, 0.0f);
	XMVECTOR tempY  = XMLoadFloat3(&m_fpUp);
	XMVECTOR tempX  = XMLoadFloat3(&m_fpRight);
	XMVECTOR tempZ  = XMLoadFloat3(&m_fpLook);

	// Rotate up and look vector about the right vector.
	R = XMMatrixRotationAxis(XMLoadFloat3(&m_fpRight), phi);

	// Transform the Up and Look basis vectors by the rotation matrix.
	XMStoreFloat3(&m_fpUp,	 XMVector3TransformNormal(XMLoadFloat3(&m_fpUp),   R));
	XMStoreFloat3(&m_fpLook, XMVector3TransformNormal(XMLoadFloat3(&m_fpLook), R));

	XMVECTOR tempAngle = XMVector3AngleBetweenVectors(tempUp, XMLoadFloat3(&m_fpUp));

	float angle = XMVectorGetX(tempAngle) * (180 / XM_PI);	// Angle between Up vector and fpUp vector.
	if (angle > m_fpPhiClampMax || angle < m_fpPhiClampMin)
	{
		XMStoreFloat3(&m_fpLook,  tempZ);
		XMStoreFloat3(&m_fpRight, tempX);
		XMStoreFloat3(&m_fpUp,	  tempY);
	}
}

//-------------------------------------------------------------------------------------
void Camera::FPStrafe(float d)
{
	// Make camera move along right basis vector.
	// m_position += d*m_right

	XMVECTOR s	= XMVectorReplicate(d);					// Replicate d in all four vector slots.
	XMVECTOR r	= XMLoadFloat3(&m_fpRight);				// Store camera right basis vector.
	XMVECTOR p	= XMLoadFloat3(&m_position);			// Store camera's position.
		
	// Update camera position.
	XMStoreFloat3(&m_position, XMVectorMultiplyAdd(s, r, p));

	// Update character's position with camera increments.

	XMVECTOR vPosInc = XMVectorMultiply(s, r);			// Get position increments.
	XMFLOAT3 fPosInc;									// Load increments in float3.
	XMStoreFloat3(&fPosInc, vPosInc);

	XMFLOAT3 fCharPos = m_character->GetTranslation();	// Get character position.
	fCharPos.x += fPosInc.x;							// Increment character position.
	fCharPos.y += fPosInc.y;
	fCharPos.z += fPosInc.z;

	m_character->SetTranslation(fCharPos.x, fCharPos.y, fCharPos.z);	// Update character position.
}

//-------------------------------------------------------------------------------------
void Camera::FPStrafe(float d, XMFLOAT3& currentVelocity)
{
	// FUNCTION: Updates character rotation and calculates strafe velocity based on camera's look vector.
	
	float magnitude = d * -1;

	XMVECTOR vL = XMLoadFloat3(&m_fpRight);			// Load STRAFE look vector for first-person view.
	XMVECTOR vD = XMVectorReplicate(magnitude);		// Replicate d in the 4 slots.

	XMFLOAT3 P  = m_character->GetTranslation();	// Get character position;
	XMVECTOR XMVoldPosition = XMLoadFloat3(&P);		// Put position in vector.
	XMVECTOR vP = XMLoadFloat3(&P);					// Put position in another vector.

	XMVECTOR XMVnewPosition = XMVectorMultiplyAdd(vD, vL, vP);	// Calculate new character position.
	XMVECTOR XMVnewVelocity = XMVectorSubtract(XMVnewPosition, XMVoldPosition);	// Calculate new velocity.
		
	XMVECTOR XMVcurrentVelocity = XMLoadFloat3(&currentVelocity);			// Store passed in current velocity.
	XMVcurrentVelocity = XMVectorAdd(XMVcurrentVelocity, XMVnewVelocity);	// Add new velocity to current.
	
	XMStoreFloat3(&currentVelocity, XMVcurrentVelocity);	// Update passed in velocity parameter.
}

//-------------------------------------------------------------------------------------
void Camera::FPWalk(float d)
{
	// Make camera move along look vector.X
	// m_position += d * m_look

	// Make camera move in the X and Z axis, not the Y.
	XMVECTOR s		= XMVectorReplicate(d);						// Replicate d in all four vector slots.
	XMFLOAT3 tempL	= XMFLOAT3(m_fpLook.x, 0.0f, m_fpLook.z);	// Get the look vector's X and Z values.
	XMVECTOR l		= XMLoadFloat3(&tempL);						// Store look vector.
	XMVECTOR p		= XMLoadFloat3(&m_position);				// Get camera position.
	
	// Update camera position.
	XMStoreFloat3(&m_position, XMVectorMultiplyAdd(s, l, p));

	// Update character's position with camera increments.
	XMVECTOR vPosInc = XMVectorMultiply(s, l);					// Get position increments.
	XMFLOAT3 fPosInc;											// Load increments in float3.
	XMStoreFloat3(&fPosInc, vPosInc);

	XMFLOAT3 fCharPos = m_character->GetTranslation();			// Get character position.
	fCharPos.x += fPosInc.x;									// Increment character position.
	fCharPos.y += fPosInc.y;
	fCharPos.z += fPosInc.z;

	// Update character position.
	m_character->SetTranslation(fCharPos.x, fCharPos.y, fCharPos.z);
}

//-------------------------------------------------------------------------------------
void Camera::FPWalk(float d, XMFLOAT3& currentVelocity)
{
	// FUNCTION: Updates character rotation and calculates walk velocity based on camera's look vector.

	// Snap character rotation with camera's rotation.
	m_characterRotY = (m_theta * (180.0f / XM_PI));
	m_characterRotY *= -1;
	m_character->SetRotation(0.0f, m_characterRotY, 0.0f);

	// Get first person look vector.
	XMVECTOR vL = XMVector3Normalize(XMLoadFloat3(&m_fpLook));	// Walk look direction.

	// Calculate new character position.
	XMVECTOR vD = XMVectorReplicate(d);							// Replicate d in the 4 slots.

	XMFLOAT3 P  = m_character->GetTranslation();				// Get character position.
	XMVECTOR XMVoldPosition = XMLoadFloat3(&P);					// Store old position
	XMVECTOR vP = XMLoadFloat3(&P);								// Load character position.

	XMVECTOR XMVnewPosition = XMVectorMultiplyAdd(vD, vL, vP);	// Calculate new character position.
	XMVECTOR XMVnewVelocity = XMVectorSubtract(XMVnewPosition, 
		XMVoldPosition);										// Calculate new character velocity
		
	XMVECTOR XMVcurrentVelocity = XMLoadFloat3(&currentVelocity);
	XMVcurrentVelocity = XMVectorAdd(XMVcurrentVelocity, XMVnewVelocity);
	XMStoreFloat3(&currentVelocity, XMVcurrentVelocity);
}